/*
 * 
 * Copyright 2006 POB-Technology. All rights reserved.
 * POB-Technology PROPRIETARY/CONFIDENTIAL.
 * 
 */
#include <pob-eye.h>

#include "pattern.h"

static void InitPobProto (void);
static void MoveBot(UInt8 Way);
static int Compare(UInt8 *str, UInt8 *command, int len);
static void DrawVision (RGBFrame *FrameFromCam);

#define MOVE_RUN		0x05
#define MOVE_BACK		0x0A
#define MOVE_LEFT		0x09
#define MOVE_RIGHT		0x06
#define STOP_BOT		0

//command
#define COM_POB          "POB"
#define COM_CAM          "CAM"

#define COM_COMPRESS_ON  "ZI1"
#define COM_COMPRESS_OFF "ZI0"

#define COM_RGB          "RGB"
#define COM_HSL          "HSL"

#define COM_RED          "RED"
#define COM_GREEN        "GRE"
#define COM_BLUE         "BLU"
#define COM_HUE          "HUE"
#define COM_SAT          "SAT"
#define COM_LIG          "LIG"

#define COM_CROSS		 "CRO"

#define COM_SERVO        "SER"


#define COM_UP           "UP"
#define COM_DOWN         "DO"
#define COM_LEFT         "LE"
#define COM_RIGHT        "RI"


//if you a POB-PROTO board or a POB-BOT
#define POB_BOARD_INIT() InitPobProto(); SwitchOnAllServo()
#define POB_BOARD_MOVE() SetServoMotor(buffer[1],buffer[2])

//if you have POB-Servo board :
//#define POB_BOARD_INIT() InitPOBServo()
//#define POB_BOARD_MOVE() SetPOBServo(buffer[1],buffer[2],1)


#define INIT_POB_PROTO()  if( PobProtoConnected == 0 ){ POB_BOARD_INIT();PobProtoConnected = 1 ;}

#define INIT_POB_LCD()    if( PobLcdConnected == 0 ) { InitLCD(); InitGraphicBuffer(&LCD_Fast_Buffer_Video,64,64,EIGHT_BITS,LCD_Fast_Buffer); }

//lcd fast buffer video
static UInt8 LCD_Fast_Buffer[64*64*BYTES];
static GraphicBuffer LCD_Fast_Buffer_Video;


int main( void )
{
	int i ;
	UInt8 buffer[3] ;
	int len ;
	int ComponentOk = 0 ;
	int CompressedComponentOk = 0;

	int PobProtoConnected = 0 ;

	RGBFrame FrameFromCam;  //struct of three pointers on the RGB components
	HSLFrame FrameFromCam2 ; //HSL Frame

	int PobLcdConnected = 0;
	
	//identify variable
	UInt8 Nb_Identify=0;
	Form  ListOfForm[MAX_OF_FORM];
	UInt8 Value=0;
	UInt8 stopLoop=0;
	UInt8 data;

	UInt8 *rgb_ptr ;
	UInt8 compressed_byte;

	int wh = WIDTH_FRAME*HEIGHT_FRAME;

	//system initialization 
	InitPOBEYE();

	InitUART((UInt16)(BR_115200|NO_PARITY|ONE_STOP_BIT|LENGTH_8_BIT));


	//get the pointer of the red,green and blue video buffer
	GetPointerOnRGBFrame(&FrameFromCam);

	//get the pointer on the HSL frame buffer
	GetPointerOnHSLFrame(&FrameFromCam2);

	while(1)
	{
		len = 3 ;
		GetBufferFromUART( buffer, len );
		
		//test if pob-eye ok
		if( Compare( buffer, COM_POB,3) == 0 )
		{
			SendBufferToUART( buffer, 3 );
			continue ;
		}

		//send parameter to the pob-eye camera
		if( Compare(buffer,COM_CAM,3) == 0 )
		{
			buffer[0] = 'O' ;
			buffer[1] = 'K' ;
			SendBufferToUART( buffer,2);

			len = 3 ;
			GetBufferFromUART( buffer, len );

			//saturation
			if( buffer[0] == 'S' && buffer[1] == 'A' )
			{
				SendToCam( 0x03, buffer[2] );
			}
			//contrast
			else if( buffer[0] == 'C' && buffer[1] == 'O' )
			{
				SendToCam( 0x05, buffer[2] );
			}
			//brightness
			else if( buffer[0] == 'B' && buffer[1] == 'R' )
			{
				SendToCam( 0x06, buffer[2] );
			}

			for(i=0;i<0xFFFF;i++);	
			for(i=0;i<0xFFFF;i++);

			continue ;
		}

		//send command to servomotor
		//run only if you have a pob-proto board !
		if( Compare(buffer,COM_SERVO,3)==0)
		{
			if( PobProtoConnected == 0 )
			{
				POB_BOARD_INIT();
				PobProtoConnected = 1 ;
			}

			buffer[0] = 'O' ;
			buffer[1] = 'K' ;
			SendBufferToUART( buffer,2);

			len = 3 ;
			GetBufferFromUART( buffer, len );
			//V v1 v2

			//servo motor move order
			//SetServoMotor(buffer[1],buffer[2]);
			POB_BOARD_MOVE();

			buffer[0] = 'O' ;
			buffer[1] = 'K' ;
			SendBufferToUART( buffer,2);
			continue ;
		}

		//move pob-bot tank, 
		//run only if you have pob-bot ! crash pob-eye if you don't have pob-bot
		if( Compare(buffer,COM_UP,2)==0)
		{
			INIT_POB_PROTO();
			MoveBot( MOVE_RUN );
			Wait(100000);
			MoveBot( STOP_BOT );

			buffer[0] = 'O' ;
			buffer[1] = 'K' ;
			SendBufferToUART( buffer,2);
			continue ;
		}
		else if( Compare(buffer,COM_DOWN,2)==0)
		{
			INIT_POB_PROTO();
			MoveBot( MOVE_BACK );
			Wait(100000);
			MoveBot( STOP_BOT );

			buffer[0] = 'O' ;
			buffer[1] = 'K' ;
			SendBufferToUART( buffer,2);
			continue ;
		}
		else if( Compare(buffer,COM_LEFT,2)==0)
		{
			INIT_POB_PROTO();
			MoveBot( MOVE_LEFT );
			Wait(100000);
			MoveBot( STOP_BOT );

			buffer[0] = 'O' ;
			buffer[1] = 'K' ;
			SendBufferToUART( buffer,2);
			continue ;
		}
		else if( Compare(buffer,COM_RIGHT,2)==0)
		{
			INIT_POB_PROTO();
			MoveBot( MOVE_RIGHT );
			Wait(100000);
			MoveBot( STOP_BOT );

			buffer[0] = 'O' ;
			buffer[1] = 'K' ;
			SendBufferToUART( buffer,2);
			continue ;
		}

		
		//grab a new frame
		if( Compare(buffer,COM_RGB,3)==0)
		{
			GrabRGBFrame();
			continue ;
		}

		if( Compare(buffer, COM_CROSS,3)==0)
		{
			INIT_POB_LCD();
			INIT_POB_PROTO();

			ClearGraphicBuffer(&LCD_Fast_Buffer_Video);

			do
			{
				stopLoop=GetByteFromUARTNoWait(&data);
				GrabRGBFrame();	
				BinaryRGBFrame(&FrameFromCam);						
				DrawVision(&FrameFromCam);							

				Nb_Identify=IdentifyForm(&FrameFromCam,ListOfForm,pattern);								
							
				Value=STOP_BOT;
				for(i=0;i<Nb_Identify;i++)
				{
					if(ListOfForm[i].id==IDP_CROSS)
					{								
						if(ListOfForm[i].x>55)				
						{
							Value=MOVE_RIGHT;
						}
						else if(ListOfForm[i].x<35)
						{
							Value=MOVE_LEFT;
						}
						else
						{
							Value=MOVE_RUN;
						}
					}							

					if (Value==MOVE_RIGHT||Value==MOVE_LEFT)
					{
						MoveBot(Value);
						Wait(50000);
						Value=STOP_BOT;
					}
				}

				MoveBot(Value);

			}while(stopLoop==0);


			continue;
		}

		//compress rgb buffer before send
		if( Compare(buffer, COM_COMPRESS_ON, 3)==0)
		{
			CompressedComponentOk=1;
			continue;
		}
		if( Compare(buffer, COM_COMPRESS_OFF, 3)==0)
		{
			CompressedComponentOk=0;
			continue;
		}

		//grab a new HSL frame
		if( Compare(buffer,COM_HSL,3)==0)
		{
			GrabRGBFrame();
			GetHSLFrame(&FrameFromCam2);
			continue ;
		}

		//get RGB video buffer
		if( Compare(buffer,COM_RED,3)==0)
		{
			rgb_ptr = FrameFromCam.red;
			ComponentOk = 1 ;
		}
		else if( Compare(buffer,COM_GREEN,3)==0)
		{
			rgb_ptr = FrameFromCam.green;
			ComponentOk = 1 ;
		}
		else if( Compare(buffer,COM_BLUE,3)==0)
		{
			rgb_ptr = FrameFromCam.blue;
			ComponentOk = 1 ;
		}

		if( ComponentOk==0)
		{
			//get HSL video buffer
			if( Compare(buffer,COM_HUE,3)==0)
			{
				rgb_ptr = FrameFromCam2.hue;
				ComponentOk = 1 ;
			}
			if( Compare(buffer,COM_SAT,3)==0)
			{
				rgb_ptr = FrameFromCam2.saturation;
				ComponentOk = 1 ;
			}
			if( Compare(buffer,COM_LIG,3)==0)
			{
				rgb_ptr = FrameFromCam2.light;
				ComponentOk = 1 ;
			}
		}

		if( ComponentOk == 1 && CompressedComponentOk == 0)
		{
			//send buffer
			for (i=0; i < wh ; i++)
			{
				SendByteToUART( rgb_ptr[i]);
			
				//pob-eye crash if you send to many byte
				if( i%500 == 0 )
					GetByteFromUART();
			}

			ComponentOk = 0 ;
		}

		if( ComponentOk == 1 && CompressedComponentOk == 1)
		{
			//send buffer
			for (i=0; i < wh ; i=i+2)
			{
				compressed_byte = (rgb_ptr[i]&0xF0) | ( rgb_ptr[i+1]>>4);
			
				SendByteToUART(compressed_byte);
			
				//pob-eye crash if you send to many byte
				if( i%500 == 0 )
					GetByteFromUART();
			}

			ComponentOk=0;
		}

	}

	return 0;
}


//Function to initialize the POB-PROTO board
void InitPobProto (void)
{
	PobProto	Proto; // struct to set the pob-proto
	
	Proto.porta=ALL_PORTA_AS_ANA;	//to get the position of the analogic joystick, you have to set the PORTA as analogic input
	Proto.portc=RC7_AS_SERVO	| RC6_AS_SERVO |RC3_AS_SERVO |RC2_AS_SERVO|RC1_AS_SERVO |RC0_AS_SERVO;	//all pin of PORTC are configured to manage servomotors
	Proto.portd=RD7_AS_DI	| RD6_AS_DI	|RD5_AS_DI |RD4_AS_DI|RD3_AS_DO	|RD2_AS_DO	|RD1_AS_DO	|RD0_AS_DO;		//RD0 RD1 RD2 RD3 are configured as digitals output to gear DC motor, RD4 RD5 RD6 RD7 are configured as digitals input
	SetPobProto(&Proto);	//set the pob proto

}


//function to move the bot
void MoveBot (UInt8 Way)
{	
	SetPortD(Way);
}

int Compare(UInt8 *str, UInt8 *command, int len)
{
	int i=0;

	while( i < len )
	{
		if( str[i] != command[i] )
			return -1;

		i++;
	}

	return 0;
}

//Draw the frame binarised 
void DrawVision (RGBFrame *FrameFromCam)
{	
	//draw the red buffer (88*120 pixels) on the Buffer for for the left screen (64*64 pixels)
	DrawComponentInABufferVideo(FrameFromCam->red,&LCD_Fast_Buffer_Video);
	DrawLeftLCD(&LCD_Fast_Buffer_Video); //refresh the left screen
}
